/*
 * ComplexFloatSVDDecomp.cs
 * Managed code is a port of JAMA and Jampack code.
 * Copyright (c) 2003-2004, dnAnalytics Project. All rights reserved.
*/

using System;
using dnA.Exceptions;
using System.Runtime.InteropServices;

namespace dnA.Math {
	///<summary>This class computes the SVD factorization of a general <c>ComplexFloatMatrix</c>.</summary>
	public sealed class ComplexFloatSVDDecomp : Algorithm{
		private ComplexFloatMatrix u;
		private ComplexFloatMatrix v;
		private FloatMatrix w;
		private ComplexFloatMatrix X;
		private readonly bool computeVectors;
		private const int MAXITER = 1000;
		
		/*///<summary>Returns the left singular vectors.</summary>
		///<returns>the left singular vectors.</returns>
		public ComplexFloatMatrix U {
			get{
				Compute();
				return u;
			}
		}*/

		/*///<summary>Returns the right singular vectors.</summary>
		///<returns>the right singular vectors.</returns>
		public ComplexFloatMatrix V{
			get{
				Compute();
				return v;
			}
		}*/

		///<summary>Returns the singular values as a diagonal matrix.</summary>
		///<returns>the singular values as a diagonal matrix.</returns>
		public FloatMatrix S {
			get{
				Compute();
				return w;
			}
		}

		///<summary>Returns the two norm of the matrix.</summary>
		///<returns>the two norm of the matrix.</returns>
		public float Norm2{
			get{
				Compute();
				return w[0,0];
			}
		}

		///<summary>Returns the condition number <c>max(S) / min(S)</c>.</summary>
		///<returns>the condition number.</returns>
		public float Condition {
			get{
				Compute();
				int tmp = System.Math.Min(X.RowLength,X.ColumnLength)-1;
				return w[0,0]/w[tmp,tmp];
			}
		}

		///<summary>Returns the effective numerical matrix rank>.</summary>
		///<returns>the number of nonnegligible singular values.</returns>
		public int Rank{
			get{
				Compute();
				float eps = (float)System.Math.Pow(2.0,-52.0);
				float tol = (float)System.Math.Max(X.RowLength,X.ColumnLength)*w[0,0]*eps;
				int r = 0;
				for (int i = 0; i < w.RowLength; i++) {
					if (w[i,i] > tol) {
						r++;
					}
				}
				return r;
			}
		}

		/*///<summary>Constructor for SVD decomposition class.</summary>
		///<param name="matrix">The matrix to decompose.</param>
		///<param name="computeVectors">Whether to compute the singular vectors or not.</param>
		///<exception cref="ArgumentNullException">matrix is null.</exception>
		public ComplexFloatSVDDecomp(ComplexFloatMatrix matrix, bool computeVectors){
			if ( matrix == null ) {
				throw new System.ArgumentNullException("matrix cannot be null.");
			}
			this.X = matrix.Clone();
			this.computeVectors = computeVectors;
		}*/

		///<summary>Constructor for SVD decomposition class.</summary>
		///<param name="matrix">The matrix to decompose.</param>
		///<exception cref="ArgumentNullException">matrix is null.</exception>
		public ComplexFloatSVDDecomp(ComplexFloatMatrix matrix){
			if ( matrix == null ) {
				throw new System.ArgumentNullException("matrix cannot be null.");
			}
			this.X = matrix.Clone();
			computeVectors = false;
		}

		///<summary>Computes the algorithm.</summary>
		protected override void InternalCompute(){
#if MANAGED
			//PORT of ZSVD from JAMPACK
			/*    Initialization */
			ComplexFloat scale = ComplexFloat.Zero;
			ComplexFloat zr = ComplexFloat.Zero;

			int mc = System.Math.Min(X.RowLength, X.ColumnLength);
			float[] d = new float[mc];
			float[] e = new float[mc];
			ComplexFloatMatrix S = new ComplexFloatMatrix(mc);
			u = ComplexFloatMatrix.CreateIdentity(X.RowLength);
			v = ComplexFloatMatrix.CreateIdentity(X.ColumnLength);
			int rx = X.RowLength - 1;  
			int cx = X.ColumnLength - 1;
			int vrx = v.RowLength - 1;
			int urx = u.RowLength - 1;
			int vcx = v.ColumnLength - 1;
			int ucx = u.ColumnLength - 1;
			int m = System.Math.Min(rx, cx);

			/*Reduction to Bidiagonal form.*/
			ComplexFloatVector h = null;
			ComplexFloatVector temp = new ComplexFloatVector(System.Math.Max(X.RowLength,X.ColumnLength));
			for (int k=0; k<=m; k++){
				h = Householder.GenerateColumn(X, k, rx, k);
				Householder.UA(h, X, k, rx, k+1, cx, temp);
				Householder.AU(u, h, 0, urx, k, ucx, temp);
				if (k != cx){
					h = Householder.GenerateRow(X, k, k+1, cx);
					Householder.AU(X, h, k+1, rx, k+1, cx, temp);
					Householder.AU(v, h, 0, vrx, k+1, vcx, temp);
				}
			}

			/*Scale the bidiagonal matrix so that its elements are real.*/

			ComplexFloat xkk1 = ComplexFloat.Zero;
			for (int k=0; k<=m; k++){
				int kk = k-0;
				ComplexFloat xkk = X[k,k];
				float axkk = ComplexMath.Absolute(xkk);
				X[k,k] = axkk;
				d[kk] = axkk;
				scale = ComplexMath.Conjugate(xkk) / axkk;
				if (k<cx){
					xkk1 = X[k,k+1];
					xkk1 *= scale;
					X[k, k+1] = xkk1;
				}
				scale = ComplexMath.Conjugate(scale);
				for (int i=0; i<=urx; i++){
					zr = u[i,k] * scale;
					u[i,k] = zr;
				}

				if (k<cx){
					xkk1 = X[k,k+1];
					float axkk1 = ComplexMath.Absolute(xkk1);
					X[k,k+1] = axkk1;
					e[kk] = axkk1;
					scale = ComplexMath.Conjugate(xkk1) / axkk1;
					if (k<rx){
						ComplexFloat xk1k1 = X[k+1,k+1];
						xk1k1 *= scale;						
						X[k+1, k+1] =  xk1k1;
					}
					for (int i=0; i<=vrx; i++){
						zr = v[i,k+1] * scale;
						v[i, k+1] = zr;
					}
				}
			}

			m = m - 0;  // Zero based loops from here on.
			/* If X has more columns than rows, rotate out the extra superdiagonal element.	*/
			CFRot P = new CFRot();
			if (X.RowLength < X.ColumnLength){
				float t = e[m];
				for (int k=m; k>=0; k--){
					CFRot.genr(d[k], t, P);
					d[k] = P.zr;
					if (k != 0){
						t = P.sr*e[k-1];
						e[k-1] = P.c*e[k-1];
					}
					CFRot.ap(v, P, 0, vrx, k+0, rx+1);
					CFRot.ap(X, P, 0, rx, k+0, rx+1);
				}
			}
			/* Calculate the singular values of the bidiagonal matrix.*/
			int iu = m;
			int iter = 0;
			while (true){
				/*These two loops determine the rows (il to iu) to iterate on.*/
				while (iu > 0){
					if (System.Math.Abs(e[iu-1]) > 1.0e-16*(System.Math.Abs(d[iu]) + System.Math.Abs(d[iu-1]))){
						break;
					}
					e[iu-1] = 0;
					iter = 0;
					iu = iu - 1;
				}
				iter = iter+1;
				if (iter > MAXITER){
					//do something?
					break;
					//throw new System.ApplicationException("Maximum number of iterations exceeded.");
				}
				if (iu == 0) {
					break;
				}

				int il = iu-1;
				while(il > 0){
					if(System.Math.Abs(e[il-1]) <=	1.0e-16*(System.Math.Abs(d[il]) + System.Math.Abs(d[il-1]))){
						break;
					}
					il = il-1;
				}
				if (il != 0){
					e[il-1] = 0.0f;
				}
				/*Compute the shift (formulas adapted from LAPACK).*/
				float dmax = System.Math.Max(System.Math.Abs(d[iu]), System.Math.Abs(d[iu-1]));
				float dmin = System.Math.Min(System.Math.Abs(d[iu]), System.Math.Abs(d[iu-1]));
				float ea = System.Math.Abs(e[iu-1]);
				float ass = 0.0f;
				float shift = 0.0f;
				float au = 0;
				float at = 0;
				float t = 0;
				if (dmin == 0.0f){
					shift = 0.0f;
				}else if(ea < dmax){
					ass = 1.0f + dmin/dmax;
					at = (dmax-dmin)/dmax;
					au = ea/dmax;
					au = au*au;
					shift =dmin*(2.0f/(float)((System.Math.Sqrt(ass*ass+au) + System.Math.Sqrt(at*at+au))));
				}else{
					au = dmax/ea;
					if (au == 0.0f){
						shift = (dmin*dmax)/ea;
					}
					else{
						ass = 1.0f + dmin/dmax;
						at = (dmax-dmin)/dmax;
						t = 1.0f/((float)System.Math.Sqrt(1.0+(ass*au)*(ass*au))+(float)System.Math.Sqrt(1.0+(at*au)*(at*au)));
						shift = (t*dmin)*au;
					}
				}
				/*Perform the implicitly shifted QR step.*/
				t = (float)(System.Math.Max(System.Math.Max(System.Math.Abs(d[il]),System.Math.Abs(e[il])), shift));
				float ds = d[il]/t; 
				float es = e[il]/t; 
				float ss = shift/t;
				CFRot.genr((ds-ss)*(ds+ss), ds*es, P);
				for (int i=il; i<iu; i++){
					t = P.c*d[i] - P.sr*e[i];
					e[i] = P.sr*d[i] + P.c*e[i];
					d[i] = t;
					t = -P.sr*d[i+1];
					d[i+1] =  P.c*d[i+1];
					CFRot.ap(v, P, 0, vrx, 0+i, 0+i+1);
					CFRot.genc(d[i], t, P);
					d[i] = P.zr;
					t = P.c*e[i] + P.sr*d[i+1];
					d[i+1] = P.c*d[i+1] - P.sr*e[i];
					e[i] = t;
					CFRot.aph(u, P, 0, urx, 0+i, 0+i+1);
					if (i != iu-1){
						t = P.sr*e[i+1];
						e[i+1] = P.c*e[i+1];
						CFRot.genr(e[i], t, P);
						e[i] = P.zr;
					}
				}
			}

			/* Sort the singular values, setting negative values of d to positive.*/
			for (int k=m; k>=0; k--){
				if (d[k] < 0){
					d[k] = -d[k];
					if(computeVectors){
						for (int i=0; i<X.ColumnLength; i++){
							v[i,k] = new ComplexFloat(-v[i,k].Real, -v[i,k].Imag);
						}
					}
				}
				for (int j=k; j<m; j++){
					if(d[j] < d[j+1]){
						float t = d[j];
						d[j] = d[j+1];
						d[j+1] = t;
						if(computeVectors){
							for (int i=0; i<X.RowLength; i++){
								ComplexFloat tmp  = u[i,j];
								u[i,j] = u[i,j+1];
								u[i,j+1] = tmp;
							}
							for (int i=0; i<X.ColumnLength; i++){
								ComplexFloat tmp = v[i,j];
								v[i,j] = v[i,j+1];
								v[i,j+1] = tmp;
							}
						}
					}
				}
			}
#else
			int m = X.RowLength;
			int n = X.ColumnLength;
			float[] d = new float[System.Math.Min(m+1,n)];
			int nu = System.Math.Min(m,n);
			u = new ComplexFloatMatrix(m,nu);
			v = new ComplexFloatMatrix(n);
			ComplexFloat[] a = new ComplexFloat[X.data.Length];
			Array.Copy(X.data, a, X.data.Length);
			dnA.Math.Lapack.Gesvd.Compute(m, n, a, d, u.data, v.data );
			v.Transpose();
#endif		
			int slen = System.Math.Min(X.RowLength,X.ColumnLength);
			w = new FloatMatrix(slen,X.ColumnLength);
			for ( int i = 0; i < slen; i++ ) {
				w[i,i] = d[i];
			}
			if( !computeVectors ){
				u = null;
				v = null;
			}
		}

		private class CFRot{
			public float c;
			public float sr;
			public float si;
			public float zr;
			public float zi;

			public static CFRot genc(float xr, float xi, float yr, float yi){
				float s, absx, absxy;
				CFRot P = new CFRot();

				if (xr == 0 && xi==0){
					P.c = 0.0f;
					P.sr = 1.0f;
					P.si = 0.0f;
					P.zr = yr;
					P.zi = yi;
					return P;
				}
				s = (float)(System.Math.Abs(xr) + System.Math.Abs(xi));
				absx = s*(float)System.Math.Sqrt((xr/s)*(xr/s) + (xi/s)*(xi/s));
				s = (float)(System.Math.Abs(s) + System.Math.Abs(yr) + System.Math.Abs(yi));
				absxy = s*(float)System.Math.Sqrt((absx/s)*(absx/s) + (yr/s)*(yr/s) + (yi/s)*(yi/s));
				P.c = absx/absxy;
				xr = xr/absx;
				xi = xi/absx;
				P.sr = (xr*yr + xi*yi)/absxy;
				P.si = (xi*yr - xr*yi)/absxy;
				P.zr = xr*absxy;
				P.zi = xi*absxy;
				return P;
			}

			public static void genc(float xr, float xi, float yr, float yi, CFRot P){
				float s, absx, absxy;
				if (xr == 0 && xi==0){
					P.c = 0.0f;
					P.sr = 1.0f;
					P.si = 0.0f;
					P.zr = yr;
					P.zi = yi;
					return;
				}
				s = (float)(System.Math.Abs(xr) + System.Math.Abs(xi));
				absx = s*(float)System.Math.Sqrt((xr/s)*(xr/s) + (xi/s)*(xi/s));
				s = (float)(System.Math.Abs(s) + System.Math.Abs(yr) + System.Math.Abs(yi));
				absxy = s*(float)System.Math.Sqrt((absx/s)*(absx/s) + (yr/s)*(yr/s) + (yi/s)*(yi/s));
				P.c = absx/absxy;
				xr = xr/absx;
				xi = xi/absx;
				P.sr = (xr*yr + xi*yi)/absxy;
				P.si = (xi*yr - xr*yi)/absxy;
				P.zr = xr*absxy;
				P.zi = xi*absxy;
			}

			public static CFRot genc(float x, float y){
				CFRot P = new CFRot();
				P.si = 0.0f;
				P.zi = 0.0f;

				if (x==0 & y==0){
					P.c = 1;
					P.sr = 0.0f;
					P.zr = 0.0f;
					return P;
				}

				float s = (float)(System.Math.Abs(x) + System.Math.Abs(y));
				P.zr = s*(float)System.Math.Sqrt((x/s)*(x/s) + (y/s)*(y/s));
				P.c = x/P.zr;
				P.sr = y/P.zr;
				return P;
			}

			public static void genc(float x, float y, CFRot P){
				P.si = 0.0f;
				P.zi = 0.0f;

				if (x==0 & y==0){
					P.c = 1;
					P.sr = 0.0f;
					P.zr = 0.0f;
					return;
				}

				float s = (float)(System.Math.Abs(x) + System.Math.Abs(y));
				P.zr = s*(float)System.Math.Sqrt((x/s)*(x/s) + (y/s)*(y/s));
				P.c = x/P.zr;
				P.sr = y/P.zr;
			}

			public static CFRot genc(ComplexFloatMatrix A, int i1, int i2, int j){
				CFRot P = CFRot.genc(A[i1,j].Real, A[i1,j].Imag, A[i2,j].Real, A[i2,j].Imag);
				A[i1,j] = new ComplexFloat(P.zr,P.zi);
				A[i2,j] = ComplexFloat.Zero;
				return P;
			}

			public static void genc(ComplexFloatMatrix A, int i1, int i2, int j, CFRot P){
				CFRot.genc(A[i1,j].Real, A[i1,j].Imag, A[i2,j].Real, A[i2,j].Imag, P);
				A[i1,j] = new ComplexFloat(P.zr,P.zi);
				A[i2,j] = ComplexFloat.Zero;
			}


			public static CFRot genr(float xr, float xi, float yr, float yi){
				float s, absx, absxy;
				CFRot P = new CFRot();
				if (xr == 0 && xi==0){
					P.c = 0.0f;
					P.sr = 1.0f;
					P.si = 0.0f;
					P.zr = yr;
					P.zi = yi;
					return P;
				}
				s = (float)(System.Math.Abs(xr) + System.Math.Abs(xi));
				absx = s*(float)System.Math.Sqrt((xr/s)*(xr/s) + (xi/s)*(xi/s));
				s = (float)(System.Math.Abs(s) + System.Math.Abs(yr) + System.Math.Abs(yi));
				absxy = s*(float)System.Math.Sqrt((absx/s)*(absx/s) + (yr/s)*(yr/s) + (yi/s)*(yi/s));
				P.c = absx/absxy;
				xr = xr/absx;
				xi = xi/absx;
				P.sr = -(xr*yr + xi*yi)/absxy;
				P.si = (xi*yr - xr*yi)/absxy;
				P.zr = xr*absxy;
				P.zi = xi*absxy;
				return P;
			}

			public static void genr(float xr, float xi, float yr, float yi, CFRot P){
				float s, absx, absxy;
				if (xr == 0 && xi==0){
					P.c = 0.0f;
					P.sr = 1.0f;
					P.si = 0.0f;
					P.zr = yr;
					P.zi = yi;
					return;
				}
				s = (float)(System.Math.Abs(xr) + System.Math.Abs(xi));
				absx = s*(float)System.Math.Sqrt((xr/s)*(xr/s) + (xi/s)*(xi/s));
				s = (float)(System.Math.Abs(s) + System.Math.Abs(yr) + System.Math.Abs(yi));
				absxy = s*(float)System.Math.Sqrt((absx/s)*(absx/s) + (yr/s)*(yr/s) + (yi/s)*(yi/s));
				P.c = absx/absxy;
				xr = xr/absx;
				xi = xi/absx;
				P.sr = -(xr*yr + xi*yi)/absxy;
				P.si = (xi*yr - xr*yi)/absxy;
				P.zr = xr*absxy;
				P.zi = xi*absxy;
			}

			public static CFRot genr(ComplexFloatMatrix A, int i, int j1, int j2){
				CFRot P = CFRot.genr(A[i,j1].Real, A[i,j1].Imag, A[i,j2].Real, A[i,j2].Imag);
				A[1,j1] = new ComplexFloat(P.zr,P.zi);
				A[2,j2] = ComplexFloat.Zero;
				return P;
			}

			public static void genr(ComplexFloatMatrix A, int i, int j1, int j2, CFRot P){
				CFRot.genr(A[i,j1].Real, A[i,j1].Imag, A[i,j2].Real, A[i,j2].Imag, P);
				A[1,j1] = new ComplexFloat(P.zr,P.zi);
				A[2,j2] = ComplexFloat.Zero;
			}

			public static CFRot genr(float x, float y){
				CFRot P = new CFRot();
				P.si = 0.0f;
				P.zi = 0.0f;

				float s = System.Math.Abs(x) + System.Math.Abs(y);

				if (s == 0.0){
					P.c = 1.0f;
					P.sr = 0.0f;
					P.zr = 0.0f;
					return P;
				}

				P.zr = s*(float)System.Math.Sqrt((x/s)*(x/s) + (y/s)*(y/s));
				P.c = x/P.zr;
				P.sr = -y/P.zr;
				return P;
			}

			public static void genr(float x, float y, CFRot P){
				P.si = 0.0f;
				P.zi = 0.0f;
				float s = (float)(System.Math.Abs(x) + System.Math.Abs(y));
				if (s == 0.0){
					P.c = 1.0f;
					P.sr = 0.0f;
					P.zr = 0.0f;
					return;
				}

				P.zr = s*(float)System.Math.Sqrt((x/s)*(x/s) + (y/s)*(y/s));
				P.c = x/P.zr;
				P.sr = -y/P.zr;
			}

			public static void pa(CFRot P, ComplexFloatMatrix A, int i1, int i2, int j1, int j2 ){
				float t1r, t1i, t2r, t2i;
				for (int j=j1; j<=j2; j++){
					t1r = P.c*A[i1,j].Real + P.sr*A[i2,j].Real - P.si*A[i2,j].Imag;
					t1i = P.c*A[i1,j].Imag + P.sr*A[i2,j].Imag + P.si*A[i2,j].Real;
					t2r = P.c*A[i2,j].Real - P.sr*A[i1,j].Real - P.si*A[i1,j].Imag;
					t2i = P.c*A[i2,j].Imag - P.sr*A[i1,j].Imag + P.si*A[i1,j].Real;
					A[i1,j] = new ComplexFloat(t1r,t1i);
					A[i2,j] = new ComplexFloat(t2r,t2i);
				}  
			}

			public static void pha(CFRot P, ComplexFloatMatrix A, int i1, int i2, int j1, int j2 ){
				float t1r, t1i, t2r, t2i;
				for (int j=j1; j<=j2; j++){
					t1r = P.c*A[i1,j].Real - P.sr*A[i2,j].Real + P.si*A[i2,j].Imag;
					t1i = P.c*A[i1,j].Imag - P.sr*A[i2,j].Imag - P.si*A[i2,j].Real;
					t2r = P.c*A[i2,j].Real + P.sr*A[i1,j].Real + P.si*A[i1,j].Imag;
					t2i = P.c*A[i2,j].Imag + P.sr*A[i1,j].Imag - P.si*A[i1,j].Real;
					A[i1,j] = new ComplexFloat(t1r,t1i);
					A[i2,j] = new ComplexFloat(t2r,t2i);
				}  
			}

			public static void ap(ComplexFloatMatrix A, CFRot P, int i1, int i2, int j1, int j2 ){
				float t1r, t1i, t2r, t2i;
				for (int i=i1; i<=i2; i++){
					t1r = P.c*A[i,j1].Real + P.sr*A[i,j2].Real - P.si*A[i,j2].Imag;
					t1i = P.c*A[i,j1].Imag + P.sr*A[i,j2].Imag + P.si*A[i,j2].Real;
					t2r = P.c*A[i,j2].Real - P.sr*A[i,j1].Real - P.si*A[i,j1].Imag;
					t2i = P.c*A[i,j2].Imag - P.sr*A[i,j1].Imag + P.si*A[i,j1].Real;
					A[i,j1] = new ComplexFloat(t1r,t1i);
					A[i,j2] = new ComplexFloat(t2r,t2i);
				} 
			}

			public static void aph(ComplexFloatMatrix A, CFRot P, int i1, int i2, int j1, int j2 ){
				float t1r, t1i, t2r, t2i;
				for (int i=i1; i<=i2; i++){
					t1r = P.c*A[i,j1].Real + P.sr*A[i,j2].Real + P.si*A[i,j2].Imag;
					t1i = P.c*A[i,j1].Imag + P.sr*A[i,j2].Imag - P.si*A[i,j2].Real;
					t2r = P.c*A[i,j2].Real - P.sr*A[i,j1].Real + P.si*A[i,j1].Imag;
					t2i = P.c*A[i,j2].Imag - P.sr*A[i,j1].Imag - P.si*A[i,j1].Real;
					A[i,j1] = new ComplexFloat(t1r,t1i);
					A[i,j2] = new ComplexFloat(t2r,t2i);
				} 
			}
		}
	}
}